
//speedpin = PWM
//potPin = analogoutput
//InP** = digital input Postive ,** = motor code
//InN** = digital input Negative ,** = motor code
/*

*/
#define speedPinA1 2 //PWM A1
#define speedPinB1 4//PWM B1
#define speedPinA2 5// PWM A2
#define speedPinB2 7//PWM B2
#define speedPinC 6// PWM C


#define LpotPinX 0// analog in A1
#define LpotPinY 1// analog in B1
#define RpotPinX 3// analog in A2
#define RpotPinY 4// analog in B2
#define CpotPin 7// analog in centre motor


#define InPA1 22//motor driver InA of motor A1
#define InNA1 23//motor driver InB of motor A1

#define InPB1 24//motor driver InA of motor A2
#define InNB1 25//motor driver InB of motor A2

#define InPA2 28//motor driver InA of motor A1
#define InNA2 29//motor driver InB of motor A1

 #define InPB2 30//motor driver InA of motor A2
#define InNB2 31//motor driver InB of motor A2

#define InPC 40// motor driver InA of motor C
#define InNC 41// motor driver InB of motor C


void calspeed(int loc, int centenrvalue ,float *realspeed, float range);
void currentflow(int value,int inA,int inB);
void ReadLoc(int potPinX, int potPinY,int *MslocX,int *MslocY);
void Readmotion(int joy,int *RmlocX,int *RmlocY,float *rangeAx, float *rangeBx);

  float LspeedA = 0;// speed A1
  float LspeedB= 0;// speed B1
  float RspeedA = 0;// speed A2
  float RspeedB = 0;// speed B2
  float speedC = 0;
  
int LcentX = 518;
int LcentY = 509;
int K = 999;



void setup() {
  
/*pinMode(InPA1, OUTPUT);
pinMode(InPB1, OUTPUT);
pinMode(InNA1, OUTPUT);
pinMode(InNB1, OUTPUT);

pinMode(InPA2, OUTPUT);
pinMode(InNA2, OUTPUT);
pinMode(InPB2, OUTPUT);
pinMode(InNB2, OUTPUT);*/

pinMode(InPC, OUTPUT);
pinMode(InNC, OUTPUT);

/*pinMode(speedPinA1, OUTPUT);
pinMode(speedPinB1, OUTPUT);
pinMode(5, OUTPUT);
pinMode(7, OUTPUT);*/
pinMode(speedPinC,OUTPUT); 


Serial.begin(9600);
}

void loop() {
  
  int locX = 0;
  int locY = 0;
  float rangeA = 0;
  float rangeB = 0;
  
/* Readmotion(1,&locX,&locY,&rangeA,&rangeB);//,&rangeA,&rangeB);
  calspeed(locX,LcentX,&LspeedA);//,rangeA);
  calspeed(locY,LcentY,&LspeedB);//,rangeB*/
  
/* Readmotion(2,&locX,&locY,&rangeA,&rangeB);
  calspeed(locX,LcentX,&RspeedA,rangeA);
  calspeed(locY,LcentY,&RspeedB,rangeB);*/
  
  Readmotion(3,&locX,&locY,&rangeA,&rangeB);
   calspeed(locX,512,&speedC,rangeA);
  
  
  
  Serial.print(locX);
    Serial.print("         ");
    Serial.print(locY);
    Serial.print("         ");
     
  
  /* analogWrite (speedPinA1,LspeedA);
   analogWrite (speedPinB1,LspeedB);
   analogWrite (5,RspeedA);
   analogWrite (7,RspeedB);*/
   analogWrite(speedPinC,speedC);
    Serial.print("\n");
   
   delay(50);

  
  
}


void calspeed(int loc, int centenrvalue ,float *realspeed ,float range)
{
  if (loc <= centenrvalue ) {//motor rotate clockwise
  *realspeed = (( centenrvalue - loc)/range)*255;//diff as the speed, "/2"is because the range is already halved once
}  
  else if(loc > centenrvalue ) {
  *realspeed = (( loc - centenrvalue)/range)*255;
  }
      Serial.print(*realspeed);
       Serial.print("         ");
      

}

// 1 forward 2 backward 3 stop
void currentflow(int value,int inA,int inB)
{
  switch(value)
  {
    case 1:
          digitalWrite(inA, HIGH);
          digitalWrite(inB, LOW);
          break;
    case 2:
          digitalWrite(inB, HIGH);
          digitalWrite(inA, LOW);
          break;
  /*  case 3:
          digitalWrite(inB, LOW);
          digitalWrite(inA, LOW);
          break;*/
    default:
          Serial.print("Bye Bye");
          break;
      
  }  
}

void ReadLoc(int potPinX, int potPinY,int *MslocX,int *MslocY)
{
  *MslocX = analogRead(potPinX);
  *MslocY = analogRead(potPinY);
}


void Readmotion(int joy ,int *RmlocX,int *RmlocY,float *rangeAx, float *rangeBx)/*,int *rangeAx ,int *rangeBx)*///joy=joystick (1L 2R)
{
   *RmlocX = 0;
   *RmlocY = 0;
  
      Serial.print(joy);
      Serial.print("         ");
 switch(joy)
     {
          case 1:
               ReadLoc(LpotPinX,LpotPinY,&(*RmlocX),&(*RmlocY));   
                 if(*RmlocY >= 509)
              {
                currentflow(2,InPB1,InNB1);            
       //     *rangeBx = 32; 
              }
              else
              {
                     currentflow(1,InPB1,InNB1);
           // *rangeBx = 512;
                    }
            if(*RmlocX >=518) 
              {
                 currentflow(1,InPA1,InNA1);
           //     *rangeAx = ;
              }
             else 
               {
                  currentflow(2,InPA1,InNA1);
             //   *rangeAx = InNB159;
               }
               break;
   
         case 0:    ReadLoc(RpotPinX,RpotPinY,&(*RmlocX),&( *RmlocY));   
                       if(*RmlocY>= 509)
                     {
                        currentflow(1,InPB2,InNB2); //move forward           
                        *rangeBx = 386; 
                     }
                        else
                     {
                             currentflow(2,InPB2,InNB2);//move backward
                            *rangeBx = 517;
                     }
                           if(*RmlocX >=517) 
                          {
                             currentflow(1,InPA2,InNA2); //move forward 
                              *rangeAx = 469 ;
                          }
                               else 
                           {
                              currentflow(2,InPA2,InNA2);//move backward
                               *rangeAx =455;
                           }
                 break;
    
    
         case 3:       ReadLoc(CpotPin,K,&(*RmlocX),&( *RmlocY));  
                       if(*RmlocX >=512) 
                          {
                             currentflow(1,InPC,InNC); //move forward 
                              *rangeAx = 514 ;
                          }
                               else 
                           {
                              currentflow(2,InPC,InNC);//move backward
                               *rangeAx =514;
                           }
     }
}
